#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include <time.h>

#include "opencv2/opencv.hpp"

using namespace cv;
using namespace std;

int find_median(float a[5])
{
	int swap;
	for (int c = 0 ; c < 5; c++)
	{
		for (int d = 0 ; d < 4- c ; d++)
			{
				 if (a[d] > a[d+1]) /* For decreasing order use < */
				 {
					  swap   = a[d];
					  a[d]   = a[d+1];
					  a[d+1] = swap;
				}
			}
	}
	return(a[2]);
}

int main(int argc,char** argv)
{
	//////////////////////////////////////////////////////////////// Variable Declaration //////////////////////////////////////////////////////////
	VideoCapture cap(argv[1]); // open the file
    if(!cap.isOpened())  // check if we succeeded
        return -1;

	if (argc != 3)
    {
        cout << "Not enough parameters" << endl;
        return -1;
    }

    const string source      = argv[1];           // the source file name
    const bool askOutputType = argv[2][0] =='Y';  // If false it will use the inputs codec type

    VideoCapture inputVideo(source);              // Open input
    if (!inputVideo.isOpened())
    {
        cout  << "Could not open the input video: " << source << endl;
        return -1;
    }

    string::size_type pAt = source.find_last_of('.');                  // Find extension point
    const string NAME = source.substr(0, pAt) + "_bg" + ".avi";   // Form the new name with container
    int ex = static_cast<int>(inputVideo.get(CV_CAP_PROP_FOURCC));     // Get Codec Type- Int form

    // Transform from int to char via Bitwise operators
    char EXT[] = {(char)(ex & 0XFF) , (char)((ex & 0XFF00) >> 8),(char)((ex & 0XFF0000) >> 16),(char)((ex & 0XFF000000) >> 24), 0};

    Size S = Size((int) inputVideo.get(CV_CAP_PROP_FRAME_WIDTH),    // Acquire input size
                  (int) inputVideo.get(CV_CAP_PROP_FRAME_HEIGHT));

    VideoWriter outputVideo;                                        // Open the output
    if (askOutputType)
        outputVideo.open(NAME, ex=-1, inputVideo.get(CV_CAP_PROP_FPS), S, true);
    else
        outputVideo.open(NAME, ex, inputVideo.get(CV_CAP_PROP_FPS), S, true);

    if (!outputVideo.isOpened())
    {
        cout  << "Could not open the output video for write: " << source << endl;
        return -1;
    }

    cout << "Input frame resolution: Width=" << S.width << "  Height=" << S.height << " of nr#: " << inputVideo.get(CV_CAP_PROP_FRAME_COUNT) << endl;
    cout << "Input codec type: " << EXT << endl;

	/////////////////////////////////////////////////////////////// Background Extraction ////////////////////////////////////////////////////////
	
	Mat src, res;
	Mat cur_temp=Mat::zeros(S,CV_8UC1);
	Mat prev_temp=Mat::zeros(S,CV_8UC1);
   	Mat next_temp=Mat::zeros(S,CV_8UC1);
	Mat prev_temp2=Mat::zeros(S,CV_8UC1);
	Mat next_temp2=Mat::zeros(S,CV_8UC1);
	Mat background = Mat::zeros(S,CV_8UC1);
	uchar* data_cur=(uchar*)cur_temp.data;
	uchar* data_prev=(uchar*)prev_temp.data;
	uchar* data_prev2=(uchar*)prev_temp2.data;
	uchar* data_next=(uchar*)next_temp.data;
	uchar* data_next2=(uchar*)next_temp2.data;
	uchar* data_background = (uchar*)background.data;
	float a[5],img_median;int width_step=cur_temp.step1();
   
    for(;;) //Show the image captured in the window and repeat
    {
        inputVideo >> src;              // read
        if (src.empty()) break;         // check if at end
		cvtColor(src,cur_temp, CV_BGR2GRAY);
		cvtColor(src,next_temp2, CV_BGR2GRAY);

		for(int i=0;i<S.height;i++)
		{
			for(int j=0;j<S.width;j++)
			{
				a[0]=data_prev2[i*width_step+j];
				a[1]=data_prev[i*width_step+j];
				a[2]=data_cur[i*width_step+j];
				a[3]=data_next[i*width_step+j];
				a[4]=data_next2[i*width_step+j];

				img_median=find_median(a);
				data_background[i*width_step+j]=img_median;
			}
		}
      	
		prev_temp.copyTo(prev_temp2);
        cur_temp.copyTo(prev_temp);
        next_temp.copyTo(cur_temp);
		next_temp2.copyTo(next_temp);

	    background.copyTo(res);
        outputVideo << res;
    }

    cout << "Finished writing" << endl;
    return 0;
}

